#include "simstruc.h"
#include "tmwtypes.h"
#include <libserialport.h>
#include <pthread.h>
#include <stdio.h>
#include <unistd.h> 
#include "os/types.h"
#include "os/os.h"
#include "app/app.h"
#include "controller/motor.h"
#include "utils/circle_buffer.h"
#include "utils/logger.h"

#define UART_NAME "COM1"

extern void SysTick_Handler(void);

FILE * fp_debug;

static c_buffer_t *uart_queue = NULL;
static hw_call_back uart_cb = NULL;
static void *       uart_ctx = NULL;
static struct sp_port *com_port = NULL;
static pthread_t thread_1ms, thread_os;
static volatile boolean_T thread_running = false;
static void start_os_threads(void);


void msleep(u32 ms) {
    useconds_t us = ms * 1000;
    usleep(us);
}

static void serial_port_open(void) {        
    sp_get_port_by_name(UART_NAME, &com_port);
    if (com_port == NULL) {
        return;
    }
    sp_open(com_port, SP_MODE_READ_WRITE);
    sp_set_baudrate(com_port, 500000);
    sp_set_bits(com_port, 8);
    sp_set_parity(com_port, SP_PARITY_NONE);
    sp_set_stopbits(com_port, 1);
    sp_set_flowcontrol(com_port, SP_FLOWCONTROL_NONE);
}

static void serial_port_close(void) {
    if (com_port) {
        sp_close(com_port);
        sp_free_port(com_port);
    }
}

static void serial_read(void) {
    u8 rx_buff[16];
    int len = sp_blocking_read(com_port, rx_buff, sizeof(rx_buff), 1);
    if (len > 0) {
        if (uart_cb != NULL) {
            uart_cb(uart_ctx, rx_buff, len);
        }
    }
}

static void serial_write(void) {
    u8 tx_buff[300];
    u16 len = circle_get_bytes(uart_queue, tx_buff, sizeof(tx_buff));
    if (len > 0) {
        sp_blocking_write(com_port, tx_buff, len, 1);
    }
}

void uart_init(unsigned int rate, hw_call_back cb, void *param) {
    uart_ctx = param;
    uart_cb = cb;
    serial_port_open();
    if (com_port) {
        ssPrintf("Serial Port COM1 is opened\n");
        thread_running = true;
        start_os_threads();
    }
}
void uart_set_dma_queue(void *queue) {
    uart_queue = (c_buffer_t *)queue;
}
void uart_tx_poll(void) {
    serial_write();
}
void uart_rx_poll(void) {
    serial_read();
}


static void *work_task_1ms(void *p) {
    while(thread_running) {
        SysTick_Handler();
        msleep(1);
    }
	fprintf(fp_debug, "sys 1ms exit\n");
	fflush(fp_debug);
    return NULL;
}

static void *work_task_os(void *p) {
    while(thread_running) {
        os_task_run();
        usleep(10);
    }
    fprintf(fp_debug, "Close Com1\n");
	fflush(fp_debug);
    serial_port_close();
    return NULL;
}

static void start_os_threads(void) {
    pthread_create(&thread_1ms, NULL, work_task_1ms, NULL);
    pthread_create(&thread_os, NULL, work_task_os, NULL);
}


void sim_sfunc_terminate(SimStruct *S) {
    ssPrintf("Stop\n");
	thread_running = false;
	motor(0)->samples.b_get_offset = false;
    msleep(100);
    if (fp_debug) {
        fflush(fp_debug);
        fclose(fp_debug);
    }
}

void sim_sfunc_init(SimStruct *S) {
    if (fp_debug == NULL) {
        fp_debug = fopen("debug.txt", "w+");
    }
    app_start();
}
